#include <stdio.h>
#include <termios.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/time.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include "motor_drv.h"
#include"main.hpp"


std::ofstream outFile;
MotorAttr_t tMotorAttr = {              //电机基本属性配置
    .ratedTorque = 5.0f,
    .maxSpeed = 193.0f,
    .encoderResolution = ENCODER_PLUSE_ROUND,
    .reductionRatio = 54.4,
    .maxAcc = 2000.0f,               //最大加速度,RPM/s
    .maxDec = 2000.0f,               //最大减速度,RPM/s
    .stopDec = 2000.0f,              //快停减速度,RPM/s
};

char nonblocking_input(void)
{
    struct termios old_settings, new_settings;
    int ch;
 
    // 获取当前的终端设置
    tcgetattr(0, &old_settings);
    new_settings = old_settings;
 
    // 设置非阻塞模式
    new_settings.c_lflag &= ~ICANON;
    new_settings.c_cc[VMIN] = 0;
    new_settings.c_cc[VTIME] = 0;
 
    // 应用新的终端设置
    tcsetattr(0, TCSANOW, &new_settings);
 
    // 循环读取按键输入
    ch = getchar();
    
    // 恢复原来的终端设置
    tcsetattr(0, TCSANOW, &old_settings);

    return ch;
}


int main(int argc, char *argv[])
{
    outFile.open("ss88.txt");
    if (!outFile.is_open()) {  
        std::cerr << "无法打开文件！" << std::endl;  
        return 1; // 如果文件无法打开，则退出程序  
    }
    outFile << "start_record:"<<std::endl;
    outFile << "tgtPos,tgtSpeed,curPos,curSpeed,curTor,cmdSpeed\n";


    double kp,ki,kd;
    int ret;
    struct timeval tv;
    MotorDrv Motor;
    char key;

    if(argc < 2)
    {
        printf("CAN device select error,please try again");
        exit(EXIT_FAILURE);
    }

    ret = Motor.init(argv[1], &tMotorAttr);
    if(ret)
    {
        exit(EXIT_FAILURE);
    }

    ret = Motor.useSpeedMode();  //实时读取传感器输出的数据
    if(ret)
    {
        exit(EXIT_FAILURE);
    }
    printf("开始运行,按q可停止:\n");
    while(1)
    {

    kp=ini["kp"];
    // std::cout<<"kp_zhhw: "<<kp<<std::endl;

        if(Motor.test())
        {
            break;
        }
        key = nonblocking_input();
        if(key == 'q')          //按q停止
        {
            break;
        }
    }
    
    printf("exit\n");
    /* 退出 */
    Motor.exit();
    exit(EXIT_SUCCESS);
    outFile.close();
}

